
from arm_manager import ArmManager
from gripper_manager import GripperManager
import rospy

if __name__ == "__main__":

    rospy.init_node('arm_gripper', anonymous= False)
    cmarm = ArmManager()
    cmgripper = GripperManager()
    armp1 = [0,0,0,0,0]
    armp2 = [0.0,0.98,0,0.61,0]
    armp3 = [0.22,0.98,0,0.61,0]
    armp4 = [0.42,0.98,0,0.61,0]
    speed = 0.13
    cmarm.set_velocity(armp2,speed)
    rospy.sleep(5)
    cmarm.set_velocity(armp4,speed)
    rospy.sleep(5)

    cmgripper.set_velocity(0.5,0.15)
    rospy.sleep(5)

    cmarm.set_velocity(armp3,speed)
    # rospy.sleep(5)

